package net.guruqu.rvoView;

import java.util.HashMap;
import java.util.Iterator;
import java.util.LinkedList;

import net.guruqu.rvoSystem.RVOSimulator;
import net.guruqu.rvoSystem.Vector2;

import com.jme.math.Quaternion;
import com.jme.math.Vector2f;
import com.jme.scene.Node;
import com.jme.scene.Spatial.LightCombineMode;

/**
 * @author Jasper
 * 
 * Class is going to act as a model of the simulation whose responsibility is
 * keep the scene node information updated according to the RVO System ( Because
 * RVO system only provides update and query functions) This class acts as a
 * adapter layer, which update the underlying RVO system as well as the scene
 * node infomration.
 */
public class SimulationAdapter {
	float groundLevel;
	RvoSetupInfo info;
	Node renderNode;
	HashMap<Integer, SceneAgent> sceneAgents;
	RVOSimulator simulator;

	public SimulationAdapter() {
		renderNode = new Node("Rvo Simulation Node");
		sceneAgents = new HashMap<Integer, SceneAgent>();
		groundLevel = 0;
	}

	public float getGroundLevel() {
		return groundLevel;
	}

	public Node getRenderNode() {
		return renderNode;
	}

	public RVOSimulator getSimulator() {
		return simulator;
	}

	public void initScenario(RvoSetupInfo info) {
//		renderNode.setLightCombineMode(LightCombineMode.Off);

		this.info = info;
		renderNode.detachAllChildren();
		sceneAgents.clear();
//		if (simulator != null) {
//			simulator.delete();
//		}
		
		simulator = new RVOSimulator();
		setGroundLevel(info.getGroundLevel());
		simulator.setTimeStep(0.25f);
		simulator.setAgentDefaults(250, 125.0f, 40, 4.0f, 20.0f, 1.0f, 2.0f,
				55f, 1.0f);
		simulator.setRoadmapAutomatic(true);
		for (MapAgent ma : info.getMapAgents()) {
			int id = simulator.addAgent(new Vector2(ma.getPosition().x, ma
					.getPosition().y), simulator.addGoal(new Vector2(ma
					.getGoal().x, ma.getGoal().y)));
			SceneAgent agent = new SceneAgent(id);
			agent.setGroundLevel(groundLevel);
			agent.setPosition(ma.getPosition().x, ma.getPosition().y);
			sceneAgents.put(id, agent);
			renderNode.attachChild(agent);
		}

		for (LinkedList<Vector2f> v2f : info.getRoadMaps()) {
			Iterator<Vector2f> i = v2f.iterator();
			Vector2f last = i.next();
			while (i.hasNext()) {
				Vector2f cur = i.next();
				simulator.addObstacle(new Vector2(last.x, last.y), new Vector2(
						cur.x, cur.y));
				last = cur;
			}

			simulator.addObstacle(new Vector2(last.x, last.y), new Vector2(v2f
					.get(0).x, v2f.get(0).y));
		}
		LinkedList<Vector2f> cps = info.getControlPoints();
		HashMap<Vector2f, Integer> v2i = new HashMap<Vector2f, Integer>();
		for (Vector2f v2f : info.getControlPoints()) {
			int id = simulator.addRoadmapVertex(new Vector2(v2f.x, v2f.y));
			v2i.put(v2f, id);
		}

		for (Vector2f[] i : info.getControlPointsEdge()) {
			int id1 = v2i.get(i[0]);
			int id2 = v2i.get(i[1]);
			simulator.addRoadmapEdge(id1, id2);
		}
		simulator.initSimulation();
		update(0.1f);
		renderNode.updateModelBound();
		renderNode.updateRenderState();
	}

	public void restart() {
		if (info == null)
			return;
		initScenario(info);
	}

	public void setGroundLevel(float groundLevel) {
		this.groundLevel = groundLevel;
		for (SceneAgent agent : sceneAgents.values()) {
			agent.setGroundLevel(groundLevel);
		}
	}

	public boolean update(float ts) {
		if (simulator == null)
			return true;
		simulator.setTimeStep(ts);
		simulator.doStep();
		
		for (Integer key : sceneAgents.keySet()) {
			Vector2 v2 = simulator.getAgentPosition(key);
			SceneAgent agent = sceneAgents.get(key);
			agent.setPosition(v2.x(), v2.y());

			float orientation=simulator.getAgentOrientation(key);
			Quaternion quaternion = new Quaternion(new float[]{0,-orientation,0});
			agent.setLocalRotation(quaternion);
			// System.out.println("Agent#"+key+" -- "+" X:"+v2.x()+"
			// Y:"+v2.y());
			// System.out.println("Agent#"+key+" -- "+"
			// X:"+agent.getLocalTranslation().x+"
			// Y:"+agent.getLocalTranslation().z);
		}
		renderNode.updateModelBound();
		renderNode.updateGeometricState(0.01f, true);
		return simulator.getReachedGoal();
	}

	public HashMap<Integer, SceneAgent> getSceneAgents() {
		return sceneAgents;
	}
}
